#include <Adafruit_PWMServoDriver.h>
#include <ESP8266WiFi.h>
#include <Adafruit_ST7735.h> // Hardware-specific library for ST7735
#include <EEPROM.h>

#define led 2 //发光二极管连接在8266的GPIO2上
#define MAX_SRV_CLIENTS 3   //最大同时联接数，即你想要接入的设备数量，8266tcpserver只能接入五个
//锂电池电压检测，分压电路设计，ModeMCU有二次分压，开发模块ADC输入电压为0 ~ 3.3V
//锂电池正极 → R1 (10kΩ) ──┬── A0引脚  
//                   │  
//                   R2 (30kΩ) ── GND  
//                   │  
//              0.1μF电容 ── GND（滤波）
const int adcPin = A0;  // 假设使用A0引脚
const float R1 = 9.92;  // 单位kΩ
const float R2 = 29.89;  // 单位kΩ
const float internal_ratio = 0.3125;  // NodeMCU内部分压比
const float vRef = 0.9782; // ESP8266 ADC参考电压（通常为1.0V）

//#define wifi
#define ps2
#define TFT

#ifdef TFT
// 定义自定义引脚（软件 SPI）
#define TFT_SCLK 5 // D1 GPIO5
#define TFT_MOSI 4 // D2 GPIO4
#define TFT_CS 16   // D0 GPIO16
#define TFT_RST 0  // D3 GPIO0
#define TFT_DC 2   // D4 GPIO2
// For 1.44" and 1.8" TFT with ST7735 use:
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);
float lx=0,ly=0,rx=0,ry=0;
#endif

#ifdef ps2
#include <PS2X_lib.h>  //for v1.6

#define PS2_DAT        13  //13    
#define PS2_CMD        15  //15
#define PS2_SEL        14  //14
#define PS2_CLK        12  //12

#define pressures   true
//#define pressures   false
#define rumble      true
//#define rumble      false

PS2X ps2x; // create PS2 Controller Class

int error = 0;
byte type = 0;
byte vibrate = 0;
int count = 10;
#endif

char cmd = 'e';
const char *ssid     = "Flynn-Chen";//这里写入网络的ssid
const char *password = "Chenfei@2809";//wifi密码

WiFiServer server(8266);
WiFiClient serverClients[MAX_SRV_CLIENTS];

void setup()
{
  Serial.begin(115200);
  pinMode(led, OUTPUT);
  digitalWrite(led, 1);
//  analogReadResolution(10); // 设置ADC为10位分辨率（0~1023），ESP8266不需要此语句，默认为10bit，0 ~ 1023
#ifdef wifi
  WiFi.begin(ssid, password);
  Serial.println("");

  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }
  server.begin();

  Serial.println("");
  Serial.print("Connected to ");
  Serial.println(ssid);
  Serial.print("IP address: "); // 打印访问的地址
  Serial.println(WiFi.localIP());
#endif

#ifdef ps2
  while (count > 0)
  {
    delay(1000); // 1 second wait
    count--;
    Serial.printf("Find PS2 Controller :%d --------------------------",count);
    Serial.println();
    // setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
    if (error == 0)
    {
      Serial.print("Found Controller, configured successful ");
      Serial.print("pressures = ");
      if (pressures)
        Serial.print("true");
      else
        Serial.print("false");
      Serial.print(", rumble = ");
      if (rumble)
        Serial.println("true");
      else
        Serial.println("false");
      Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
      Serial.println("holding L1 or R1 will print out the analog stick values.");
      Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
      count = 0;
    }
    else if (error == 1)
      Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

    else if (error == 2)
      Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

    else if (error == 3)
      Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  }

  type = ps2x.readType();
  switch (type)
  {
  case 0:
    Serial.println("Unknown Controller type found ");
    break;
  case 1:
    Serial.println("DualShock Controller found ");
    break;
  case 2:
    Serial.println("GuitarHero Controller found ");
    break;
  case 3:
    Serial.println("Wireless Sony DualShock Controller found ");
    break;
  }
#endif

#ifdef TFT
tft.initR(INITR_BLACKTAB);
tft.setRotation(3);
tft.fillScreen(ST7735_BLACK);
tft.setTextColor(ST7735_WHITE);
tft.drawRoundRect(1, 1, 79, 79, 5, ST77XX_YELLOW);
tft.fillCircle(40,40,3,ST7735_RED);
tft.drawRoundRect(81, 1, 79, 79, 5, ST77XX_GREEN);
tft.fillCircle(120,40,3,ST7735_RED);
#endif
}

void loop() {
  // 读取ADC值（取10次平均值）
  int adcValue = 0;
  for (int i = 0; i < 10; i++) {
    adcValue += analogRead(adcPin);
    delay(10);
  }
  adcValue /= 10;

  // 计算电压
  float vOut = (adcValue / 1023.0) * vRef; // ADC值转电压
  float vBat = vOut * (R1 + R2) / R2 / internal_ratio;      // 反推锂电池电压

  // 打印结果
  Serial.print("ADC Value: ");
  Serial.print(adcValue);
  Serial.print(" | Vbat: ");
  Serial.print(vBat, 3);
  Serial.println("V");
  delay(1000);
#ifdef wifi
  uint8_t i;
  if (server.hasClient())
  {
    for (i = 0; i < MAX_SRV_CLIENTS; i++)
    {
      if (!serverClients[i] || !serverClients[i].connected())
      {
        if (serverClients[i])
          serverClients[i].stop();             // 未联接,就释放
        serverClients[i] = server.available(); // 分配新的
        continue;
      }
    }
    WiFiClient serverClient = server.available();
    serverClient.stop();
  }
  for (i = 0; i < MAX_SRV_CLIENTS; i++)
  {
    if (serverClients[i] && serverClients[i].connected())
    {
      digitalWrite(led, 0);
      if (serverClients[i].available())
      {
        while (serverClients[i].available())
        {
          Serial.println(serverClients[i].read());
        }
      }
    }
  }
#endif

#ifdef ps2
  if (error == 1) // skip loop if no controller found
    return;

  if (type == 2)
  {                      // Guitar Hero Controller
    ps2x.read_gamepad(); // read controller

    if (ps2x.ButtonPressed(GREEN_FRET))
      Serial.println("Green Fret Pressed");
    if (ps2x.ButtonPressed(RED_FRET))
      Serial.println("Red Fret Pressed");
    if (ps2x.ButtonPressed(YELLOW_FRET))
      Serial.println("Yellow Fret Pressed");
    if (ps2x.ButtonPressed(BLUE_FRET))
      Serial.println("Blue Fret Pressed");
    if (ps2x.ButtonPressed(ORANGE_FRET))
      Serial.println("Orange Fret Pressed");

    if (ps2x.ButtonPressed(STAR_POWER))
      Serial.println("Star Power Command");

    if (ps2x.Button(UP_STRUM)) // will be TRUE as long as button is pressed
      Serial.println("Up Strum");
    if (ps2x.Button(DOWN_STRUM))
      Serial.println("DOWN Strum");

    if (ps2x.Button(PSB_START)) // will be TRUE as long as button is pressed
    {
      Serial.println("Start is being held");
      cmd = 'e';
    }

    if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");

    if (ps2x.Button(ORANGE_FRET))
    { // print stick value IF TRUE
      Serial.print("Wammy Bar Position:");
      Serial.println(ps2x.Analog(WHAMMY_BAR), DEC);
    }
  }
  else
  {                                    // DualShock Controller
    ps2x.read_gamepad(false, vibrate); // read controller and set large motor to spin at 'vibrate' speed

    if (ps2x.Button(PSB_START)) // will be TRUE as long as button is pressed
    {
      Serial.println("Start is being held");
      cmd = 'e';
    }
    if (ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");

    if (ps2x.Button(PSB_PAD_UP))
    { // will be TRUE as long as button is pressed
      cmd = 'a';
      Serial.print("Up held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    }
    if (ps2x.Button(PSB_PAD_RIGHT))
    {
      cmd = 'd';
      Serial.print("Right held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    }
    if (ps2x.Button(PSB_PAD_LEFT))
    {
      cmd = 'c';
      Serial.print("LEFT held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    }
    if (ps2x.Button(PSB_PAD_DOWN))
    {
      cmd = 'b';
      Serial.print("DOWN held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    }

    vibrate = ps2x.Analog(PSAB_CROSS); // this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState())
    { // will be TRUE if any button changes state (on to off, or off to on)
      if (ps2x.Button(PSB_L3))
        Serial.println("L3 pressed");
      if (ps2x.Button(PSB_R3))
        Serial.println("R3 pressed");
      if (ps2x.Button(PSB_L2))
        Serial.println("L2 pressed");
      if (ps2x.Button(PSB_R2))
        Serial.println("R2 pressed");
      if (ps2x.Button(PSB_TRIANGLE))
      {
        cmd = 'h';
        Serial.println("Triangle pressed");
      }
    }

    if (ps2x.ButtonPressed(PSB_CIRCLE)) // will be TRUE if button was JUST pressed
    {
      cmd = 'g';
      Serial.println("Circle just pressed");
    }

    if (ps2x.NewButtonState(PSB_CROSS)) // will be TRUE if button was JUST pressed OR released
    {
      cmd = 'i';
      Serial.println("X just changed");
    }

    if (ps2x.ButtonReleased(PSB_SQUARE)) // will be TRUE if button was JUST released
    {
      cmd = 'f';
      Serial.println("Square just released");
    }

    if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1))
    { // print stick values if either is TRUE
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); // Left stick, Y axis. Other options: LX, RY, RX
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC);
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC);
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC);
    }
  }
  delay(50);
#endif

#ifdef TFT
  float lx0 = 5 + (70 * ps2x.Analog(PSS_LX)) / 255;
  float ly0 = 5 + (70 * ps2x.Analog(PSS_LY)) / 255;
  float rx0 = 5 + (70 * ps2x.Analog(PSS_RX)) / 255;
  float ry0 = 5 + (70 * ps2x.Analog(PSS_RY)) / 255;
  if (lx != lx0 || ly != ly0)
  {
    tft.fillCircle(lx, ly, 3, ST7735_BLACK);
    tft.fillCircle(lx0, ly0, 3, ST7735_RED);
    tft.setTextColor(ST7735_WHITE, ST7735_BLACK);
    tft.setCursor(0,100);
    //tft.fillRect(0,100,6*17,8,ST7735_BLACK);
    tft.printf("rx=%5.2f,ry=%5.2f",lx0,ly0);
    lx = lx0;
    ly = ly0;
  }
  if (rx != rx0 || ry != ry0)
  {
    tft.fillCircle(80 + rx, ry, 3, ST7735_BLACK);
    tft.fillCircle(80 + rx0, ry0, 3, ST7735_RED);
    rx = rx0;
    ry = ry0;
  }
#endif

// 各关节舵机标定程序
while (Serial.available() > 0)
{
  char cmd = Serial.read();
  Serial.print("Serial Cmd=");
  Serial.println(cmd);
  if (cmd == 'x') // 校准舵机角度
  {
    while (Serial.available() > 0)
    {
      int servo_id = Serial.parseInt();
      Serial.read();
      int val = Serial.parseInt();
      Serial.printf("id[%d]=%d", servo_id, val);
      Serial.println();
    }
  }
  }
  
}